#include "Finger.h"

Finger::Finger(b2World* world, b2Vec2 fingerPosition, float32 fingerSize, float32 fingerDensity, float32 fingerFriction)
{
	m_world = world;
	m_fingerPosition = fingerPosition;
	m_fingerSize = fingerSize;
	m_fingerDensity = fingerDensity;
	m_fingerFriction = fingerFriction;

	b2BodyDef myBodyDef;
	myBodyDef.type = b2_dynamicBody;
	myBodyDef.position = m_fingerPosition;

	b2CircleShape fingerShape;
	fingerShape.m_radius = m_fingerSize;

	b2FixtureDef myFixtureDef;
	myFixtureDef.shape = &fingerShape;
	myFixtureDef.density = m_fingerDensity;
	myFixtureDef.friction = m_fingerFriction;

	m_fingerBody = m_world->CreateBody(&myBodyDef);
	m_fingerFixture = m_fingerBody->CreateFixture(&myFixtureDef);
	m_fingerBody->SetFixedRotation(true);
}

Finger::~Finger()
{
	if(m_fingerBody != NULL)
	{
		m_world ->DestroyBody(m_fingerBody);
		m_fingerBody = NULL;
	}
}

b2Vec2 Finger::GetFingerPosition()
{
	m_fingerPosition = m_fingerBody->GetPosition();
	return m_fingerPosition;
}

b2Vec2 Finger::GetPullPointPosition()
{
	return m_pullPointPosition;
}

void Finger::SetPullPointPosition(const b2Vec2& val)
{
	m_pullPointPosition = val;
}

b2Fixture* Finger::GetFingerFixture()
{
	return m_fingerFixture;
}

b2Body* Finger::GetFingerBody()
{
	return m_fingerBody;
}

float32 Finger::GetFingerFriction()
{
	return m_fingerFriction;
}

void Finger::SetFingerFriction(const float32& val)
{
	m_fingerFriction = val;
}

float32 Finger::GetFingerDensity()
{
	return m_fingerDensity;
}

void Finger::SetFingerDensity(const float32& val)
{
	m_fingerDensity = val;
}

float32 Finger::GetFingerSize()
{
	return m_fingerSize;
}

void Finger::SetFingerSize(const float32& val)
{
	m_fingerSize = val;
}

void Finger::DrawForce(b2Vec2 reactionForce, DebugDraw& debugDraw)
{
	b2Vec2 forceArrow = reactionForce;
	b2Vec2 fingerPosition = GetFingerPosition();

	{
		forceArrow.Normalize();
		forceArrow *= -1;
		forceArrow += fingerPosition;
		debugDraw.DrawSegment(fingerPosition, forceArrow, b2Color(0.9f, 0.9f, 0.9f));

		b2Transform xf;
		float32 angle = std::atan2(forceArrow.y - fingerPosition.y, forceArrow.x - fingerPosition.x);
		xf.q.Set(angle);
		xf.p = fingerPosition;

		b2Vec2 vertices[3];
		vertices[0] = b2Mul(xf, b2Vec2(0.0f, 0.0f));
		vertices[1] = b2Mul(xf, b2Vec2(0.15f, -0.05f));
		vertices[2] = b2Mul(xf, b2Vec2(0.1f, 0.0f));
		debugDraw.DrawPolygon(vertices, 3, b2Color(0.9f, 0.9f, 0.9f));


		vertices[0] = b2Mul(xf, b2Vec2(0.0f, 0.0f));
		vertices[1] = b2Mul(xf, b2Vec2(0.1f, 0.0f));
		vertices[2] = b2Mul(xf, b2Vec2(0.15f, 0.05f));
		debugDraw.DrawPolygon(vertices, 3, b2Color(0.9f, 0.9f, 0.9f));
	}
}

b2Vec2 Finger::GetFingerVelocity()
{
	return m_fingerBody->GetLinearVelocity();
}

b2Vec2 Finger::GetFingerReactionForce()
{
	return m_fingerReactionForce;
}

void Finger::SetFingerReactionForce(const b2Vec2& val)
{
	m_fingerReactionForce = val;
}